#include "mpu6050_reg.h"
#include "mpu6050.h"
#include "ioi2c.h"
#include "bsp/bsp.h"
#include "includes.h"

static int mpu6050_read_regs(mpu6050_t *pdev, uint8_t reg_addr, uint8_t *rx_buf, int len) {
  ioi2c_t *pi2c = pdev->bus;

  if (true == i2c_writeoffset(pi2c, MPU6050_I2C_ADDR, &reg_addr, 1)) {
    if (true == i2c_readbytes(pi2c, MPU6050_I2C_ADDR, rx_buf, len)) {
      return PRJ_E_OK;
    }
  }
  return PRJ_E_ERROR;
}

static int mpu6050_write_regs(mpu6050_t *pdev, uint8_t reg_addr, uint8_t *tx_buf, int len) {
  ioi2c_t *pi2c = pdev->bus;
  uint8_t tmp_buf[16];

  tmp_buf[0]=reg_addr;
  for(int i=0;i<len;i++){
    tmp_buf[i+1]=tx_buf[i];
  }
    if (true == i2c_writebytes(pi2c, MPU6050_I2C_ADDR, tmp_buf, len+1)) {
      return PRJ_E_OK;
    }
  return PRJ_E_ERROR;
}

int mpu6050_init(mpu6050_t *pdev) {
  uint8_t rxbuf[2] = {0};

  if (PRJ_E_OK == mpu6050_read_regs(pdev, REG_WHO_AM_I, rxbuf, 1)) {
    if (rxbuf[0] == WHO_AM_I_VALUE) {
      rxbuf[0] = 0x00;
      while (1) {
        if (PRJ_E_OK == mpu6050_read_regs(pdev, REG_PWR_MGMT_1, rxbuf, 1)) {
          if (rxbuf[0] == 0x00) {
                return PRJ_E_OK;
          }
          rxbuf[0]=0x00;
          mpu6050_write_regs(pdev, REG_PWR_MGMT_1, rxbuf, 1);
          bsp_delay_ms(200);
        }
      }
    }
  }
  return PRJ_E_NO_RESPONDS;
}

int mpu6050_set_cfg(mpu6050_t *pdev, mpu6050_cfg_t *pcfg) {
  uint8_t reg_values[4];

  // set clock an power on
  reg_values[0] = 0x01;
  if (PRJ_E_OK != mpu6050_write_regs(pdev, REG_PWR_MGMT_1, reg_values, 1)) {
    return PRJ_E_NO_RESPONDS;
  }

  // set cfg
  reg_values[0] = pcfg->data_rate;
  reg_values[1] = 0x07 & pcfg->filter;
  reg_values[2] = (0x03 & pcfg->gyro_range) << 3;
  reg_values[3] = (0x03 & pcfg->acc_range) << 3;

  if (PRJ_E_OK != mpu6050_write_regs(pdev, REG_SMPLRT_DIV, reg_values, 4)) {
    return PRJ_E_NO_RESPONDS;
  }

  // fifo not support yet

  // sleep not set

  return PRJ_E_OK;
}
int mpu6050_get_cfg(mpu6050_t *pdev, mpu6050_cfg_t *pcfg) {
  uint8_t reg_values[4];

  if (PRJ_E_OK != mpu6050_read_regs(pdev, REG_SMPLRT_DIV, reg_values, 4)) {
    return PRJ_E_NO_RESPONDS;
  }

  pcfg->data_rate = reg_values[0];
  pcfg->filter = reg_values[1] & 0x07;
  pcfg->gyro_range = 0x03 & (reg_values[2] >> 3);
  pcfg->acc_range = 0x03 & (reg_values[3] >> 3);

  return PRJ_E_OK;
}
int mpu6050_get_raw(mpu6050_t *pdev, mpu6050_raw_t *praw) {
  uint8_t reg_values[14];

  if (PRJ_E_OK != mpu6050_read_regs(pdev, REG_ACCEL_XOUT_H, reg_values, 14)) {
    return PRJ_E_NO_RESPONDS;
  }

  praw->acc_x = ((uint16_t)(reg_values[0]) << 8) + reg_values[1];
  praw->acc_y = ((uint16_t)(reg_values[2]) << 8) + reg_values[3];
  praw->acc_z = ((uint16_t)(reg_values[4]) << 8) + reg_values[5];
  praw->temperature = ((uint16_t)(reg_values[6]) << 8) + reg_values[7];
  praw->gyros_x = ((uint16_t)(reg_values[8]) << 8) + reg_values[9];
  praw->gyros_y = ((uint16_t)(reg_values[10]) << 8) + reg_values[11];
  praw->gyros_z = ((uint16_t)(reg_values[12]) << 8) + reg_values[13];
  return PRJ_E_OK;
}